﻿using System;
using OpenCvSharp;

namespace FLANN
{
    public class TemplateMatcher
    {

        /// <summary>
        /// 多角度模板匹配方法
        /// </summary>
        /// <param name="srcImage">待匹配图像</param>
        /// <param name="modelImage">模板图像</param>
        /// <param name="angleStart">起始角度</param>
        /// <param name="angleRange">角度范围</param>
        /// <param name="angleStep">角度步长</param>
        /// <param name="numLevels">金字塔层级</param>
        /// <param name="thresScore">得分阈值</param>
        /// <returns></returns>
        public ResultPoint CircleMatchNcc(Mat srcImage, Mat modelImage, double angleStart, double angleRange, double angleStep, int numLevels, double thresScore,int nccMethod)
        {
            double step = angleStep;
            double start = angleStart;
            double range = angleRange;
 
            //定义图片匹配所需要的参数
            int resultCols = srcImage.Cols - modelImage.Cols + 1;
            int resultRows = srcImage.Rows - modelImage.Rows + 1;
            Mat result = new Mat(resultCols, resultRows, MatType.CV_8U);
            Mat src = new Mat();
            Mat model = new Mat();
            srcImage.CopyTo(src);
            modelImage.CopyTo(model);
 
            //对模板图像和待检测图像分别进行图像金字塔下采样
            for (int i = 0; i < numLevels; i++)
            {
                Cv2.PyrDown(src, src, new Size(src.Cols / 2, src.Rows / 2));
                Cv2.PyrDown(model, model, new Size(model.Cols / 2, model.Rows / 2));
            }
 
            TemplateMatchModes matchMode = TemplateMatchModes.CCoeffNormed;
            switch (nccMethod)
            {
                case 0:
                    matchMode = TemplateMatchModes.SqDiff;
                    break;
                case 1:
                    matchMode = TemplateMatchModes.SqDiffNormed;
                    break;
                case 2:
                    matchMode = TemplateMatchModes.CCorr;
                    break;
                case 3:
                    matchMode = TemplateMatchModes.CCorrNormed;
                    break;
                case 4:
                    matchMode = TemplateMatchModes.CCoeff;
                    break;
                case 5:
                    matchMode = TemplateMatchModes.CCoeffNormed;
                    break;
            }
 
            //在没有旋转的情况下进行第一次匹配
            Cv2.MatchTemplate(src, model, result, matchMode);
            Cv2.MinMaxLoc(result, out double minVal, out double maxVal, out Point minLoc, out Point maxLoc, new Mat());
 
            Point location = maxLoc;
            double temp = maxVal;
            double angle = 0;
 
            Mat newImg;    
 
            //以最佳匹配点左右十倍角度步长进行循环匹配，直到角度步长小于参数角度步长
            if (nccMethod == 0 || nccMethod == 1)
            {
                do
                {
                    for (int i = 0; i <= (int)range / step; i++)
                    {
                        newImg = ImageRotate(model, start + step * i);
                        Cv2.MatchTemplate(src, newImg, result, matchMode);
                        Cv2.MinMaxLoc(result, out double minval, out double maxval, out Point minloc, out Point maxloc, new Mat());
                        if (maxval < temp)
                        {
                            location = maxloc;
                            temp = maxval;
                            angle = start + step * i;
                        }
                    }
                    range = step * 2;
                    start = angle - step;
                    step = step / 10;
                } while (step > angleStep);
                return new ResultPoint(location.X * Math.Pow(2, numLevels) + modelImage.Width / 2, location.Y * Math.Pow(2, numLevels) + modelImage.Height / 2, -angle, temp);
            }
            else
            {
                do
                {
                    for (int i = 0; i <= (int)range / step; i++)
                    {
                        newImg = ImageRotate(model, start + step * i);
                        Cv2.MatchTemplate(src, newImg, result, matchMode);
                        Cv2.MinMaxLoc(result, out double minval, out double maxval, out Point minloc, out Point maxloc, new Mat());
                        if (maxval > temp)
                        {
                            location = maxloc;
                            temp = maxval;
                            angle = start + step * i;
                        }
                    }
                    range = step * 2;
                    start = angle - step;
                    step = step / 10;
                } while (step > angleStep);
                if (temp > thresScore)
                {
                    return new ResultPoint(location.X * Math.Pow(2, numLevels), location.Y * Math.Pow(2, numLevels), -angle, temp);
                }
            }   
            return new ResultPoint();
        }

        public struct ResultPoint
        {
            public int X, Y;
            public double T, Score;
            public ResultPoint(double x, double y, double t, double score)
            {
                X = (int) x;
                Y = (int) y;
                T = t;
                Score = score;
            }
        }
        

        /// <summary>
        /// 图像旋转
        /// </summary>
        /// <param name="image">输入图像</param>
        /// <param name="angle">旋转的角度</param>
        /// <returns>旋转后图像</returns>
        static Mat ImageRotate(Mat image, double angle)
        {
            Mat newImg = new Mat();
            Point2f pt = new Point2f((float) image.Cols / 2, (float) image.Rows / 2);
            Mat r = Cv2.GetRotationMatrix2D(pt, angle, 1.0);
            Cv2.WarpAffine(image, newImg, r, image.Size());
            // Cv2.ImShow("new",newImg);
            // Console.WriteLine(angle);
            // Cv2.WaitKey(0);
            
            return newImg;
        }

        /// <summary>
        /// 金字塔下采样
        /// </summary>
        /// <param name="image">输入图像</param>
        /// <returns>下采样完图像</returns>
        public Mat ImagePyrDown(Mat image,int NumLevels)
        {
            for (int i = 0; i < NumLevels; i++)
            {
                Cv2.PyrDown(image, image, new Size(image.Cols / 2, image.Rows / 2));
            }
            
            return image;
        }
    }
}